package com.rockwell.sniffyhunter;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.Map;

import net.wimpi.modbus.facade.ModbusSerialMaster;
import net.wimpi.modbus.usbserial.SerialParameters;

import org.opencv.core.Point;

import com.rockwell.sniffyhunter.R;
import com.rockwell.sniffyhunter.modbus.ModbusSerialClient;
import com.rockwell.sniffyhunter.utils.ICollector;
import com.rockwell.sniffyhunter.model.DropZoneDetector;
import com.rockwell.sniffyhunter.model.LeftFirstMazeSolver;
import com.rockwell.sniffyhunter.model.Maze;
import com.rockwell.sniffyhunter.model.MazeDetector;
import com.rockwell.sniffyhunter.model.MazeExitDetector;
import com.rockwell.sniffyhunter.model.MazePath;
import com.rockwell.sniffyhunter.model.PredefinedMazeSolver;
import com.rockwell.sniffyhunter.model.RightFirstMazeSolver;
import com.rockwell.sniffyhunter.model.Robot;
import com.rockwell.sniffyhunter.model.RobotAction;
import com.rockwell.sniffyhunter.model.RobotCommander;
import com.rockwell.sniffyhunter.model.RobotJunctionLocation;
import com.rockwell.sniffyhunter.model.RobotSensors;
import com.rockwell.sniffyhunter.model.ObstacleDetector;
import com.rockwell.sniffyhunter.model.TreasureCollector;
import com.rockwell.sniffyhunter.model.TreasureDetector;
import com.rockwell.sniffyhunter.utils.DetectionIndex;
import com.rockwell.sniffyhunter.utils.IDetector;
import com.rockwell.sniffyhunter.utils.IMazeSolver;
import com.rockwell.sniffyhunter.utils.IRobotLogger;
import com.rockwell.sniffyhunter.utils.Speaker;
import com.rockwell.sniffyhunter.views.MazeCameraView;
import com.rockwell.sniffyhunter.views.OnColorBlobDetectListener;
import com.rockwell.sniffyhunter.views.OnOrientationChangeListener;

import android.media.AudioManager;
import android.media.MediaPlayer;
import android.media.RingtoneManager;
import android.net.Uri;
import android.os.Bundle;
import android.annotation.SuppressLint;
import android.app.Activity;
import android.content.Context;
import android.hardware.usb.UsbManager;
import android.text.Editable;
import android.text.SpannableString;
import android.text.TextWatcher;
import android.text.style.UnderlineSpan;
import android.view.Menu;
import android.view.View;
import android.view.View.OnClickListener;
import android.widget.Button;
import android.widget.ScrollView;
import android.widget.TextView;
import android.widget.Toast;

public class RobotActivity extends Activity {
	private Robot 			m_robot;
	private MazeCameraView	m_mazeCameraView;
    private ModbusSerialClient m_modbusMaster;
    private Speaker m_speaker;
    private ICollector m_treasureCollector;

    private IRobotLogger m_logger = new IRobotLogger() {

		@Override
		public void log(final String msg) {
			RobotActivity.this.runOnUiThread(new Runnable(){
			    public void run(){
			    	TextView logDisplay = (TextView)findViewById(R.id.text_log);
			    	logDisplay.append(msg + "\n");
			    	
			    	//if (m_speaker != null)
			    	//	m_speaker.speak(msg);
			    	//playSound(RobotActivity.this);
			    }
			});
		}

		@Override
		public void log(final Exception ex) {
			RobotActivity.this.runOnUiThread(new Runnable(){
			    public void run(){
			    	TextView logDisplay = (TextView)findViewById(R.id.text_log);
			    	
			    	if (ex.getMessage() != null) {
			    		logDisplay.append(ex.getMessage());
			    	}
			    	
			    	StackTraceElement[] trace = ex.getStackTrace();
			    	logDisplay.append(getTraceString(trace));
			    	logDisplay.append("\n");
			    }
			});
		}

		public String getTraceString(StackTraceElement[] stackTraceElements) {
		    if (stackTraceElements == null)
		        return "";
		    StringBuilder stringBuilder = new StringBuilder();
		    for (StackTraceElement element : stackTraceElements)
		        stringBuilder.append(element.toString()).append("\n");
		    return stringBuilder.toString();
		}
		
		@Override
		public void onRobotPaused() {
			RobotActivity.this.runOnUiThread(new Runnable(){
			    public void run(){
			    	Button resume = (Button)findViewById(R.id.button_pause_resume);
			    	resume.setText("\tResume");
			    	//resume.setTextColor(0x0000ff);
			    	resume.setEnabled(true);
			    	
			    	TextView robotStatus = (TextView)findViewById(R.id.text_robot_status);
			    	
			    	String underLine=new String("Pause Mode");
			    	SpannableString content = new SpannableString(underLine);
			    	content.setSpan(new UnderlineSpan(), 0, underLine.length(), 0);
			    	robotStatus.setText(underLine);
			    	
			    	if (m_speaker != null)
			    		m_speaker.speak("Please press start for me to run");
			    }
			});
		}

		@Override
		public void onRobotResumed() {
			RobotActivity.this.runOnUiThread(new Runnable(){
			    public void run(){
			    	Button resume = (Button)findViewById(R.id.button_pause_resume);
			    	resume.setText("\tPause");
			    	//resume.setTextColor(0xff0000);
			    	resume.setEnabled(true);
			    	
			    	TextView robotStatus = (TextView)findViewById(R.id.text_robot_status);
			    	
			    	String underLine=new String("Run Mode");
			    	SpannableString content = new SpannableString(underLine);
			    	content.setSpan(new UnderlineSpan(), 0, underLine.length(), 0);
			    	robotStatus.setText(underLine);
			    	
			    	if (m_speaker != null)
			    		m_speaker.speak("I'm Running");
			    }
			});
		}

		@Override
		public void onRobotIdle() {
			RobotActivity.this.runOnUiThread(new Runnable(){
			    public void run(){
			    	Button resume = (Button)findViewById(R.id.button_pause_resume);
			    	resume.setText("");
			    	resume.setEnabled(false);
			    	
			    	TextView robotStatus = (TextView)findViewById(R.id.text_robot_status);
			    	
			    	String underLine=new String("Idle Mode");
			    	SpannableString content = new SpannableString(underLine);
			    	content.setSpan(new UnderlineSpan(), 0, underLine.length(), 0);
			    	robotStatus.setText(underLine);
			    	
			    	if (m_speaker != null)
			    		m_speaker.speak("I'm restarting");
			    }
			});
		}
    };
    
	private OnOrientationChangeListener m_orientationListener = new OnOrientationChangeListener() {
        public void onOrientationChanged(float[] orientation) {
        	m_robot.setOrientation(orientation[0]);
        }
    };
    
	@SuppressLint("UseSparseArrays") @SuppressWarnings("serial")
    private Map<Integer, OnColorBlobDetectListener> m_colorDetectListeners = new HashMap<Integer, OnColorBlobDetectListener>() {{
    	// Treasure Detect Listener
    	put(DetectionIndex.TREASURE, new OnColorBlobDetectListener() {
	        public void onDetect(final int index, final boolean present, final Point center, final float radius, final int length, final int width) {
	        	RobotActivity.this.runOnUiThread(new Runnable(){
				    public void run(){
			        	m_robot.onDetect(index, present, center, radius, length, width);
			        	
			        	String msg;
						msg = String.format("T[%d,%d,%d,%s]", (int)center.x, (int)center.y, (int)radius, m_robot.m_colorDetectors.get(index).isPresent()? "T":"F");
						
			        	TextView treasurePosition = (TextView) RobotActivity.this.findViewById(R.id.text_treasure_position);
			        	treasurePosition.setText(msg);
				    }
	        	});
	        }
    	});
    	
    	// Obstacle Detect Listener
    	put(DetectionIndex.OBSTACLE, new OnColorBlobDetectListener() {
            public void onDetect(final int index, final boolean present, final Point center, final float radius, final int length, final int width) {
            	
            	RobotActivity.this.runOnUiThread(new Runnable(){
    			    public void run(){
    		        	m_robot.onDetect(index, present, center, radius, length, width);
    		        	
    		        	String msg;
    					msg = String.format("O[%d,%d,%d,%s]", (int)center.x, (int)center.y, (int)radius, m_robot.m_colorDetectors.get(index).isPresent()? "T":"F");
    					
    		        	TextView barrierPosition = (TextView) RobotActivity.this.findViewById(R.id.text_obstacle_position);
    					barrierPosition.setText(msg);
    			    }
            	});
            }
        });
    	
        // Maze 1 Detect Listener
    	put(DetectionIndex.MAZE1, new OnColorBlobDetectListener() {
            public void onDetect(final int index, final boolean present, final Point center, final float radius, final int length, final int width) {
            	
            	RobotActivity.this.runOnUiThread(new Runnable(){
    			    public void run(){
    		        	m_robot.onDetect(index, present, center, radius, length, width);
    		        	
    		        	String msg;
    					msg = String.format("M1[%d,%d,%d,%s]", (int)center.x, (int)center.y, (int)radius, m_robot.m_colorDetectors.get(index).isPresent()? "T":"F");
    					
    		        	TextView mazePosition = (TextView) RobotActivity.this.findViewById(R.id.text_barrier_position);
    		        	mazePosition.setText(msg);
    			    }
            	});
            }
        });
 
        // Maze 2 Detect Listener
    	put(DetectionIndex.MAZE2, new OnColorBlobDetectListener() {
            public void onDetect(final int index, final boolean present, final Point center, final float radius, final int length, final int width) {
            	
            	RobotActivity.this.runOnUiThread(new Runnable(){
    			    public void run(){
    		        	m_robot.onDetect(index, present, center, radius, length, width);
    		        	
    		        	String msg;
    					msg = String.format("M2[%d,%d,%d,%s]", (int)center.x, (int)center.y, (int)radius, m_robot.m_colorDetectors.get(index).isPresent()? "T":"F");
    					
    		        	TextView mazePosition = (TextView) RobotActivity.this.findViewById(R.id.text_barrier_position);
    		        	mazePosition.setText(msg);
    			    }
            	});
            }
        });
        
        // Maze 3 Detect Listener
    	put(DetectionIndex.MAZE3, new OnColorBlobDetectListener() {
            public void onDetect(final int index, final boolean present, final Point center, final float radius, final int length, final int width) {
            	
            	RobotActivity.this.runOnUiThread(new Runnable(){
    			    public void run(){
    		        	m_robot.onDetect(index, present, center, radius, length, width);
    		        	
    		        	String msg;
    					msg = String.format("M3[%d,%d,%d,%s]", (int)center.x, (int)center.y, (int)radius, m_robot.m_colorDetectors.get(index).isPresent()? "T":"F");
    					
    		        	TextView mazePosition = (TextView) RobotActivity.this.findViewById(R.id.text_barrier_position);
    		        	mazePosition.setText(msg);
    			    }
            	});
            }
        });
        
        // Drop Zone Detect Listener
    	put(DetectionIndex.DROPZONE, new OnColorBlobDetectListener() {
            public void onDetect(final int index, final boolean present, final Point center, final float radius, final int length, final int width) {
            	
            	RobotActivity.this.runOnUiThread(new Runnable(){
    			    public void run(){
    		        	m_robot.onDetect(index, present, center, radius, length, width);
    		        	
    		        	String msg;
    					msg = String.format("D[%d,%d,%d,%s]", (int)center.x, (int)center.y, (int)radius, m_robot.m_colorDetectors.get(index).isPresent()? "T":"F");
    					
    		        	TextView dropPosition = (TextView) RobotActivity.this.findViewById(R.id.text_obstacle_position);
    		        	dropPosition.setText(msg);
    			    }
            	});
            }
        });
    	
    	// Maze Exit Detect Listener
    	put(DetectionIndex.MAZE_EXIT, new OnColorBlobDetectListener() {
            public void onDetect(final int index, final boolean present, final Point center, final float radius, final int length, final int width) {
            	
            	RobotActivity.this.runOnUiThread(new Runnable(){
    			    public void run(){
    		        	m_robot.onDetect(index, present, center, radius, length, width);
    		        	
    		        	String msg;
    					msg = String.format("ME[%d,%d,%d,%s]", (int)center.x, (int)center.y, (int)radius, m_robot.m_colorDetectors.get(index).isPresent()? "T":"F");
    					
    		        	TextView dropPosition = (TextView) RobotActivity.this.findViewById(R.id.text_obstacle_position);
    		        	dropPosition.setText(msg);
    			    }
            	});
            }
        });
    };};

	@Override
	protected void onCreate(Bundle savedInstanceState) {
		super.onCreate(savedInstanceState);
		setContentView(R.layout.activity_robot);
		
		m_mazeCameraView = (MazeCameraView) this.findViewById(R.id.maze_camera_view);
		m_mazeCameraView.setTeachMode(false);
		m_mazeCameraView.setOrientationChangeListener(m_orientationListener);
		m_mazeCameraView.setDetectionListener(m_colorDetectListeners);

		SerialParameters params = new SerialParameters();
		params.setPortName("COM1");
		params.setBaudRate(19200);
		params.setDatabits(8);
		params.setParity("None");
		params.setStopbits(1);
		params.setEncoding("ascii");
		params.setEcho(false);

		UsbManager usb = (UsbManager) getSystemService(Context.USB_SERVICE);
    	m_modbusMaster = new ModbusSerialClient(usb);
		
    	final ScrollView sv = (ScrollView)findViewById(R.id.scrollview_log);
    	TextView tv = (TextView)findViewById(R.id.text_log);
    	tv.addTextChangedListener(new TextWatcher() {

			@Override
			public void afterTextChanged(Editable arg0) {
				sv.fullScroll(ScrollView.FOCUS_DOWN);
			}

			@Override
			public void beforeTextChanged(CharSequence s, int start, int count,
					int after) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void onTextChanged(CharSequence s, int start, int before,
					int count) {
				// TODO Auto-generated method stub
				
			}
    		
    	});
    	
		setupRestartButton();
		setupRobotPauseResumeButton();
	}

	@Override
	public boolean onCreateOptionsMenu(Menu menu) {
		// Inflate the menu; this adds items to the action bar if it is present.
		getMenuInflater().inflate(R.menu.robot, menu);
		return true;
	}

	@SuppressLint("UseSparseArrays") 
	@Override
	public void onStart() {
		super.onStart();

		try {
			m_speaker = new Speaker(this);
			
			double width = SettingsActivity.getRobotWidth(this);
			double length = SettingsActivity.getRobotLength(this);
			
			int startZone = SettingsActivity.getRobotStartZone(this);
			
			RobotSensors sensors = new RobotSensors(m_modbusMaster);
			RobotCommander robotCommander = new RobotCommander(m_modbusMaster);
			@SuppressWarnings("serial")
			Map<Integer, IDetector> colorDetectors = new HashMap<Integer, IDetector>() {
				{
					put(DetectionIndex.TREASURE, new TreasureDetector());
					put(DetectionIndex.OBSTACLE, new ObstacleDetector());
					put(DetectionIndex.MAZE1, new MazeDetector());
					put(DetectionIndex.MAZE2, new MazeDetector());
					put(DetectionIndex.MAZE3, new MazeDetector());
					put(DetectionIndex.DROPZONE, new DropZoneDetector());
					put(DetectionIndex.MAZE_EXIT, new MazeExitDetector());
				}
			};
			
			boolean leftFirst = SettingsActivity.leftFirst(this);
			
			IMazeSolver solver;
			IMazeSolver leftFirstSolver = new LeftFirstMazeSolver();
			IMazeSolver rightFirstSolver = new RightFirstMazeSolver();
			leftFirstSolver.addSolver(rightFirstSolver);
			
			
			if (leftFirst) {
				solver = leftFirstSolver;
			}
			else {
				solver = rightFirstSolver;
			}
			
			if (SettingsActivity.usePredefinedPath(this) && SettingsActivity.hasPredefinedPath(this)) {
				ArrayList<RobotAction> directions = new ArrayList<RobotAction>();
				
				if (MazePath.loadPredefinedPath(this, directions)) {
					solver = new PredefinedMazeSolver(directions, solver, m_logger);
				}
			}
			
			Maze maze = new Maze(30 /*lane width*/, 5 /*treasure count*/);
			m_treasureCollector = new TreasureCollector();
			
			m_robot = new Robot(width, length, 
					startZone,
					sensors, 
					robotCommander, 
					maze, 
					solver, 
					colorDetectors,
					m_treasureCollector);
			
			m_robot.PopulatePredefinedSolvers(this);
			m_robot.setStepMode(true);
			m_robot.setLogger(m_logger, 2);
			m_robot.start(true);
		}
		catch (Exception ex) {
            Toast.makeText(this, ex.getMessage(), Toast.LENGTH_SHORT).show();
		}
	}
	
	@Override
	public void onStop() {
		if (m_robot != null) {
			m_robot.quit();
		}
		if (m_speaker != null) {
			m_speaker.speak("Shutting down");
			m_speaker.shutDown();
		}
		if (m_mazeCameraView != null) {
			m_mazeCameraView.onDeactivated();
		}
		super.onStop();
	}
	
	@Override
	public void onResume() {
		super.onResume();

    	try {
    		//Robot.DETECT_FRONT_WALL_INDEX = SettingsActivity.getJunctionIndexDistance(this);
    		//Robot.LEFT_MOVEBACKWARD_WITHOUT_FRONT_WALL = SettingsActivity.getLeftJunctionMovebackDistance(this);
    		//Robot.RIGHT_MOVEBACKWARD_WITHOUT_FRONT_WALL = SettingsActivity.getRightJunctionMovebackDistance(this);
    		//Robot.UTURN_MOVE_INDEX = SettingsActivity.getUTurnMovebackDistance(this);
    		
    		if (m_robot != null) {
    			m_robot.setStepMode(SettingsActivity.useStepMode(this));
    			m_robot.setUseCompassCompensation(SettingsActivity.useCompassCompensation(this));
    			m_robot.setUseCompassCompensationDuringTurn(true);
    
    			if (m_robot != null && !(SettingsActivity.usePredefinedPath(this) && SettingsActivity.hasPredefinedPath(this))) {
	    			boolean leftFirst = SettingsActivity.leftFirst(this);
	    			
	    			IMazeSolver solver;
	    			IMazeSolver leftFirstSolver = new LeftFirstMazeSolver();
	    			IMazeSolver rightFirstSolver = new RightFirstMazeSolver();
	    			leftFirstSolver.addSolver(rightFirstSolver);
	    			
	    			
	    			if (leftFirst) {
	    				solver = leftFirstSolver;
	    			}
	    			else {
	    				solver = rightFirstSolver;
	    			}
	    			
	    			m_robot.changeSolver(solver);
    			}
    		}
    		
			m_modbusMaster.connect("COM1");
		} catch (Exception e) {
			// TODO Auto-generated catch block
			Toast.makeText(this, "Failed to connect to the modbus slave", Toast.LENGTH_LONG).show();
		}
    	
		m_mazeCameraView.onActivated();
	}
	
	@Override
    public void onPause()
    {
    	m_modbusMaster.disconnect();
        super.onPause();
        m_mazeCameraView.onDeactivated();
    }

	private void setupRestartButton() {
		Button restart = (Button)findViewById(R.id.button_restart);
		
		restart.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				if (m_robot != null) {
					if (m_robot.isPaused()) {
						m_robot.restart();
					}
				}
			}
			
		});
	}
	
	private void setupRobotPauseResumeButton() {
		final Button pauseResume = (Button)findViewById(R.id.button_pause_resume);
		
		pauseResume.setEnabled(false);
		
		pauseResume.setOnClickListener(new OnClickListener() {

			@Override
			public void onClick(View v) {
				if (m_robot != null) {
					if (m_robot.isPaused())
						m_robot.resumeRun();
					else
						m_robot.pause();
				}
			}
			
		});
	}

	public void playSound(Context context) {
		try {
			Uri soundUri = RingtoneManager.getDefaultUri(RingtoneManager.TYPE_NOTIFICATION);
			
			MediaPlayer mMediaPlayer = new MediaPlayer();
			mMediaPlayer.setDataSource(context, soundUri);
			
			final AudioManager audioManager = (AudioManager) context.getSystemService(Context.AUDIO_SERVICE);
			
			if (audioManager.getStreamVolume(AudioManager.STREAM_ALARM) != 0) {
			    mMediaPlayer.setAudioStreamType(AudioManager.STREAM_ALARM);
			    mMediaPlayer.setLooping(true);
			    mMediaPlayer.prepare();
			    mMediaPlayer.start();
			}
		} catch (Exception ex) {
            Toast.makeText(this, ex.getMessage(), Toast.LENGTH_SHORT).show();
		}
	}
}
